#!/usr/bin/python3

import rospy
import math
import tf
import tf2_ros
from geometry_msgs.msg import Pose, Twist, TransformStamped, PointStamped, Point
import time
import numpy as np
from std_msgs.msg import Float64MultiArray, Header
from sensor_msgs.msg import LaserScan, JointState
from gazebo_msgs.msg import ModelStates
from visualization_msgs.msg import Marker, MarkerArray
import copy
from tf2_geometry_msgs import tf2_geometry_msgs
import sys
from laser_dizuo.msg import NamedPoints

class measuring_system_initializer():
    def __init__(self,nodename, ns_prefix):
        rospy.init_node(nodename)
        # debug_prefix = "/" + ns_prefix + "/"
        debug_prefix = ""

        #parameter initialization for ros_control
        self.laser_num = rospy.get_param(debug_prefix + "laser_group_config/single_laser_num")

        joint_state_controller = {"type":"joint_state_controller/JointStateController", 
            "publish_rate": 50.0}  
        rospy.set_param(debug_prefix + "joint_state_controller", joint_state_controller)

        ptp_controller = {"type": "position_controllers/JointGroupPositionController"}
        ptp_controller['joints'] = []
        for i in range(self.laser_num):
            ptp_controller['joints'].append("laser" + str(i) + "_joint1")
            ptp_controller['joints'].append("laser" + str(i) + "_joint2")
        rospy.set_param(debug_prefix + "ptp_controller", ptp_controller)

    def run(self):
        rospy.spin()

if __name__ == '__main__':
    prefix = "measuring_system"

    if(len(sys.argv)>1):
        prefix = sys.argv[1]
        print("file written to prefix",prefix)

    server = measuring_system_initializer('measuring_system_initializer',prefix)
    server.run()
